﻿using UnityEngine;

namespace Pathfinding
{
    public class Octree
    {
        public readonly OctreeNode RootNode;
        public readonly Graph<OctreeNode, int> NavGraph; //寻路网格图

        private static void Encapsulate(GameObject gameObject, Bounds baseCube)
        {
            var coll = gameObject.GetComponent<Collider>();
            if (coll)
            {
                baseCube.Encapsulate(coll.bounds);
                Debug.Log(coll + "---" + baseCube.extents);
            }
            foreach (Transform child in gameObject.transform)
            {
                Encapsulate(child.gameObject, baseCube);
            }
        }

        public Octree(Collider[] allColliders, Graph<OctreeNode, int> navGraph)
        {
            var baseCube = new Bounds();
            NavGraph = navGraph;
            foreach (var o in allColliders)
            {
                // Encapsulate(o, baseCube);
                baseCube.Encapsulate(o.bounds);
            }

            //选取最长的一条边来作为正方体的边长，并将包围盒改成正方体
            //这里为了更好设置包围盒，同样记录半尺寸
            var cubeHalfSize = 0.5f * Mathf.Max(baseCube.size.x, baseCube.size.y, baseCube.size.z) * Vector3.one;
            baseCube.SetMinMax(baseCube.center - cubeHalfSize, baseCube.center + cubeHalfSize);

            RootNode = new OctreeNode(baseCube, null);
            foreach (var o in allColliders) //具体分裂
            {
                //有碰撞体的物体才有检测的必要
                if (o.TryGetComponent(out Collider collider))
                {
                    RootNode.Divide(collider);
                }
            }

            NodeToGraph(RootNode);
            Debug.Log(NavGraph.NodeSet.Count); //查看结点数量
            GenerateEdges();
            Debug.Log(NavGraph.EdgeList.Count); //查看边的数量
        }

        //将树中的所有结点入图
        private void NodeToGraph(OctreeNode node)
        {
            if (node == null) return;
            // 没有子节点且为非障碍的结点才能入图
            if (node.Children == null && node.Type != NodeType.Obstacles)
            {
                NavGraph.AddNode(node);
            }

            if (node.Children == null) return;
            foreach (var c in node.Children)
            {
                NodeToGraph(c);
            }
        }

        //生成边
        private void GenerateEdges()
        {
            foreach (var f in NavGraph.NodeSet)
            {
                foreach (var t in NavGraph.NodeSet)
                {
                    if (f == t)
                        continue;
                    var ray = new Ray(f.NodeCube.center, t.NodeCube.center - f.NodeCube.center);
                    // 限制全连接范围
                    var maxDistance = f.NodeCube.size.y * 0.7f;
                    if (!t.NodeCube.IntersectRay(ray, out var hitDistance)) continue;
                    if (hitDistance > maxDistance)
                        continue;
                    // 添加无向边（双向），路径长度默认为1，如有需求可自行调整
                    NavGraph.AddEdge(f, t, 1);
                    NavGraph.AddEdge(t, f, 1);
                }
            }
        }

        /// <summary>
        /// 以指定节点开始搜索，寻找到与指定位置最接近的节点
        /// </summary>
        /// <param name="start">初始点</param>
        /// <param name="pos">指定位置</param>
        /// <returns>寻找到的节点，若没找到则返回根节点</returns>
        public OctreeNode GetNodeByPos(OctreeNode start, Vector3 pos)
        {
            var findNode = RootNode;
            if (start == null)
                return findNode;
            if (start.Children == null)
            {
                if (start.NodeCube.Contains(pos))
                    return start;
            }
            else
            {
                for (var i = 0; i < 8; ++i)
                {
                    findNode = GetNodeByPos(start.Children[i], pos);
                    if (findNode != RootNode)
                        return findNode;
                }
            }

            return findNode;
        }
    }
}